;;;; -*- Mode: Lisp; Package: :dreaming-tree -*-
;;;;
;;;; Collision Detection
;;;;

;;;; Classes

;; This seems unnecessary.  I think logically it is better to say
;; (defclass shape (rigid-body trimesh)
;;  ())
;; But the problem is that there is some overlap between rigid-body and trimesh,
;; notably position and orientation, so it is better to do trimesh->rigid-body->shape.
;; 
;; Another thought is to make triangle, circle, square, polygon and polyhedron subclasses
;; of rigid-body or shape.

(defclass shape (rigid-body)
  ()) 

;;;; API For Collision Detection

(defgeneric test-intersection (shape1 shape2 &optional max-time)
  (:documentation "Test for contact or interpenetration of two shapes.  If max-time is specified will assume one or both objects is moving and will return the time at which objects contact, so long as time < max-time, nil otherwise."))
(defgeneric find-intersection (shape1 shape2 &optional max-time)
  (:documentation "Return contact set information for shape1 and shape2, if they are contacting or interpenetrating.  If max-time is specified will treat objects as moving objects and will also return contact time."))
(defgeneric compute-contact-information (type time shape1 shape2)
  (:documentation "Compute collision information for the two shapes that are contacting at time time.  Called by find-intersection, who will determine the collision type."))
(defgeneric pseudo-distance (shape1 shape2)
  (:documentation "Pseudo distance function, as described in [Ebe07], for doing fast intersection tests."))
(defgeneric pseudo-distance-derivative (function t0 f0 shape1 shape2))
(defgeneric fast-no-intersection (shape1 shape2 max-time)
  (:documentation "Initial check to see if shape1 or shape2 or touching or interpenetrating."))

(defparameter *collision-types* '(unknown separated touching overlapping))

;;;; ---*--- Pseudo distance ---*---

(defmethod pseudo-distance ((s1 bounding-sphere) (s2 bounding-sphere))
  (let* ((diff (m- (center s1) (center s2)))
	 (rsum (m+ (radius s1) (radius s2)))
	 (sd (squared-magnitude diff)))
    (- (/ sd (* rsum rsum)) 1.0)))

(defmethod pseudo-distance ((s bounding-sphere) (b bounding-box))
  (let ((sd (distance-squared (center s) b))
	(r (radius s)))
    (- (/ sd (* r r)) 1.0)))

(defmethod pseudo-distance ((b bounding-box) (s bounding-sphere))
  (pseudo-distance s b))


;;; 2D

(defclass shape-2d (spatial)
  ((vertices :accessor vertices :initarg :vertices) ;; List of vertices must be counter-clockwise!
   (cached-vertex-count :reader vertex-count :initform nil)
   (cached-normals :reader normals :initform nil)
   (bsp :reader bspace-partition :initform nil)))

(defun counter-clockwise-p (vertices)
  (iter (for i from 0 below (length vertices))
	(never (if (= i (1- (length vertices)))
		   (minusp (z (cross (svref vertices i) (svref vertices 0))))
		   (minusp (z (cross (svref vertices i) (svref vertices (1+ i)))))))))

(defmethod vertex ((self shape-2d) index)
  (with-slots (vertices (n cached-vertex-count)) self
    (cond ((= index n)
	   (svref vertices 0))
	  ((= index -1)
	   (svref vertices (1- n)))
	  (t (svref vertices index)))))

(defmethod edge-vector ((self shape-2d) index)
  (with-slots (vertices (n cached-vertex-count)) self
    (cond ((= index (1- n))
	   (m- (svref vertices 0)
	       (svref vertices index)))
	  ((= index n)
	   (edge-vector self 0))
	  ((= index -1)
	   (m- (svref vertices 0)
	       (svref vertices (1- n))))
	  (t
	   (m- (svref vertices (1+ index))
	       (svref vertices index))))))

(defmethod normal ((self shape-2d) index)
  (with-slots ((normals cached-normals)) self
    (svref normals index)))

(defmethod initialize-instance :after ((self shape-2d) &rest initargs)
  (declare (ignore initargs))
  (unless (counter-clockwise-p (vertices self))
    (error "Vertices for ~A are not arranged counterclockwise." self))
  (setf (slot-value self 'cached-vertex-count) (length (vertices self)))
  (setf (slot-value self 'cached-normals)
	(apply #'vector
	       (iter:iter (for v from 0 below (vertex-count self))
			  (collect (make-vector (y (edge-vector self v))
						(- (x (edge-vector self v)))
						(z (edge-vector self v))))))))

(defmethod draw ((s shape-2d))
  (glcolor3f 1.0 1.0 1.0)
  (iter (for v in (vertices s))
	(glvertex3fv v)))
	
;; Basic shape
(defun make-square (center scale &optional (layer 0.0))
  (let ((v (vector (m* scale (make-vector  1.0  1.0 layer))
		   (m* scale (make-vector -1.0  1.0 layer))
		   (m* scale (make-vector -1.0 -1.0 layer))
		   (m* scale (make-vector  1.0 -1.0 layer)))))
    (make-instance 'shape-2d :vertices v :origin center)))



;;; 3D geometry

(defclass shape (spatial)
  ((vn :reader vertices :initarg :vertices)
   (bv :accessor bounding-volume :initarg :bounding-volume)
   (en :reader edges :initform nil)
   (nn :reader normals :initarg :normals :documentation "Array of normal vectors for faces in polyhedron.")
   (fn :reader faces :initarg :faces :documentation "Array of list of vertex indices defining faces of polyhedron.")
   (bsp :reader bspace-partition :initform nil)))

(defmethod vertex ((obj shape) index)
  (svref (vertices obj) index))

(defmethod edge-vector-from-vertices ((cp shape) (indices vector))
  (m- (vertex cp (svref indices 1)) (vertex cp (svref indices 0))))

(defmethod face ((cp shape) index)
  (svref (faces cp) index))

(defmethod normal ((cp shape) index)
  (svref (normals cp) index))

(defmethod edge-vector ((cp shape) index)
  (edge-vector-from-vertices cp (edge cp index)))

(defmethod edge ((cp shape) index)
  (svref (edges cp) index))

(defmethod faces-adjacent-to-vertex ((cp shape) vertex-idx)
  "Return the faces that are adjacent to the vertex, as specified by its index."
  (with-slots (faces) cp
    (iter (for face in-vector faces)
	  (for i below (length faces))
	  (when (find vertex-idx face)
	    (collect i)))))

(defun edge= (e1 e2)
  (or (and (= (svref e1 0) (svref e2 0))
	   (= (svref e1 1) (svref e2 1)))
      (and (= (svref e1 1) (svref e2 0))
	   (= (svref e1 0) (svref e2 1)))))

(defmethod faces-adjacent-to-edge ((cp shape) edge-idx)
  (with-slots (faces) cp
    (flet ((contains-edge (f e)
	     (iter (for i below (length f))
		   (for fedge next (if (= i (1- (length f)))
				       (vector (svref f i) (svref f 0))
				       (vector (svref f i) (svref f (1+ i)))))
		   (finding fedge such-that (edge= fedge e)))))
      (let ((e (edge cp edge-idx)))
	(iter (for face in-vector faces)
	      (for i below (length faces))
	      (when (contains-edge face e)
		(collect i)))))))

(defun unique-edge-set (faces)
  "Connect an ordered set of unique edges created from the faces in a convex polyhedron
as specified by vertex indices."
  (let ((ordered-edges (make-array (+ 2 (length faces)) :initial-element #(0 0) :fill-pointer 0 :adjustable t)))
    (flet ((unique-add (item)
	     (unless (find item ordered-edges :test #'edge=)
	       (vector-push-extend item ordered-edges))))
      (iter (for face in-vector faces)
	    (iter (for i below (length face))
		  (if (= i (1- (length face)))
		      (unique-add (vector (svref face i) (svref face 0)))
		      (unique-add (vector (svref face i) (svref face (1+ i))))))))
    ordered-edges))

			     
(defmethod initialize-instance :after ((polyh shape) &rest initargs)
  (declare (ignore initargs))
  (with-slots (faces edges vertices normals bsp) polyh
    (let ((eset (unique-edge-set faces)))
      (setf normals (make-array (length faces) :initial-element (zero-vector 3)))
      (setf edges (make-array (length eset) :initial-contents eset)) ; So we have a simple-vector
      (iter (for face in-vector faces)
	    (for i from 0)
	    (let ((v0 (svref face 0))
		  (v1 (svref face 1))
		  (v2 (svref face 2))) ;; All faces have at least three vertices.
	      (setf (svref normals i) (cross (edge polyh (list v0 v1)) (edge polyh (list v1 v2))))))
      (setf bsp (create-tree polyh)))))
		
      
(defun make-tetrahedron (p q r s)
  "Make a tetrahedron from the four vertices p q r s."
  (make-instance 'shape
		 :vertices (vector p q r s)
		 :faces (vector #(0 2 1)
				#(0 1 3)
				#(0 3 2)
				#(1 2 3))))

(defun simple-2d-polyhedron ()
  (let* ((v0 (make-vector -1.0 -1.0 0.0))
	 (v1 (make-vector 1.0 -1.0 0.0))
	 (v2 (make-vector 1.0 1.0 0.0))
	 (v3 (make-vector -1.0 1.0 0.0)))
    (make-instance 'shape
		   :vertices (vector v0 v1 v2 v3)
		   :normals '()
		   :faces (vector #(0 1 2 3)))))


;;;; ---*--- Binary Space Partitions ---*---

(defstruct bsp
  value
  left
  right)

(defstruct sphere-arc
  "A data structure used for building a bsp tree for a polyhedron.
normals: The indices of the polyhedron normals that form this arc.
edge: The index of the edge shared by the polyhedron faces making up this arc."
  normals
  edge)

(defstruct sphere-polygon
  "A data structure for representing the spherical convex polygons 
representing the partitions of a polyhedron based on its normal and edge directions."
  normals
  vertex)

#|
Algorithm for building a bsp tree for a convex polygon:

N = list of normals (stored as indices)
ni = normal i in N
A = list of arcs (stored as index pairs, i.e. A0 = (0 1))
ai = arc i in A
Nperp = edge that corresponds to the first normal in N.
discriminating value di = (dot-product nperp ni)

for each discriminating value di for each normal in N
 if di > 0
   ni is added to right normals
 else
   ni i added to left normals
for each discriminating value pair (di0 di1) for each arc in A
 if (and di0 >= 0, di1 >= 0)
   ai is added to the right arcs
 else if (and di0 < 0, di1 < 0)
   ai is added to the right arcs
 else if (di0di1 < 0)
   ai is added to left and right arcs.

make-bsp :value n0 :left (recurse on set of left normals and arcs)
                   :right (recurse on set of right normals and arcs)

|#

(defun build-2d-bsp-tree (cp normals arcs)
  "Build a binary space partition tree for computing extremal indices
when testing for polygon intersection."
  (cond ((null normals)
	 (make-bsp :value (second (first arcs))
		   :left nil
		   :right nil))
	(t (let* ((nperp (edge cp (first normals)))
		  (left-normals (remove-if #'(lambda (x)
					       (>= (dot-product nperp (normal cp x)) 0.0))
					   (rest normals)))
		  (right-normals (set-difference (rest normals) left-normals))
		  (left-arcs (remove-if #'(lambda (x)
					    (let ((di1 (dot-product nperp (normal cp (second x))))
						  (di0 (dot-product nperp (normal cp (first x)))))
					      (and (>= di1 0.0)
						   (>= di0 0.0))))
					arcs))
		  (right-arcs (remove-if #'(lambda (x)
					     (let ((di1 (dot-product nperp (normal cp (second x))))
						   (di0 (dot-product nperp (normal cp (first x)))))
					       (and (<= di1 0.0)
						    (<= di0 0.0))))
					 arcs)))
	     (format t "Normals: ~A ~A~%" left-normals right-normals)
	     (format t "Arcs: ~A ~A~%" left-arcs right-arcs)
	     (make-bsp :value (first normals)
		       :left (cond ((null left-arcs) nil)
				   (t (build-2d-bsp-tree cp left-normals left-arcs)))
		       :right (cond ((null right-arcs) nil)
				    (t (build-2d-bsp-tree cp right-normals right-arcs))))))))

(defun build-3d-bsp-tree (cp sarcs spolys)
  (cond ((null sarcs) (make-bsp :value (sphere-polygon-vertex (if (listp spolys)
								  (first spolys)
								  spolys))))
	(t (let ((e (edge-vector cp (sphere-arc-edge (first sarcs))))
		 (left-arcs nil)
		 (right-arcs nil)
		 (left-polys nil)
		 (right-polys nil))
	     (iter (for a in (rest sarcs))
		   (for n = (sphere-arc-normals a))
		   (let* ((d0 (dot-product e (normal cp (first n))))
			  (d1 (dot-product e (normal cp (second n)))))
		     (if (and (>= d0 0.0) (>= d1 0.0))
			 (push a right-arcs)
			 (if (and (<= d0 0.0) (<= d1 0.0))
			     (push a left-arcs)
			     (progn (push a left-arcs)
				    (push a right-arcs))))))
	     (iter (for s in spolys)
		   (let ((pos-count  0)
			 (neg-count 0))
		     (iter (for n in (sphere-polygon-normals s))
			   (for d = (dot-product e (normal cp n)))
			   (if (> d 0.0)
			       (incf pos-count)
			       (if (< d 0.0)
				   (incf neg-count)))
			   (until (and (> pos-count 0) (> neg-count 0))))
		     (if (and (> pos-count 0) (= neg-count 0))
			 (push s right-polys)
			 (if (and (> neg-count 0) (= pos-count 0))
			     (push s left-polys)
			     (progn (push s left-polys)
				    (push s right-polys))))))
	     (make-bsp :value (sphere-arc-edge (first sarcs))
		       :left (build-3f-bsp-tree cp left-arcs left-polys)
		       :right (build-3f-bsp-tree cp right-arcs right-polys))))))
    
		

(defmethod create-tree ((cp shape-2d))
  (let* ((n (vertex-count cp))
	 (normals (iter (for i from 0 below n)
		       (collecting i)))
	 (arcs (iter (for i from 0 below n)
		     (collecting (if (= i (1- n))
				     (list i 0)
				     (list i (1+ i)))))))
    (build-2d-bsp-tree cp normals arcs)))

(defmethod create-tree ((cp shape))
  (with-slots (bsp) cp
    (let ((arcs (iter (for i_e below (length (edges cp)))
		      (collect (make-sphere-arc :normals (faces-adjacent-to-edge cp i_e)
						:edge i_e))))
	  (polys (iter (for i_v below (length (vertices cp)))
		       (collect (make-sphere-polygon :normals (faces-adjacent-to-vertex cp i_v)
						     :vertex i_v)))))
      (setf bsp (build-3f-bsp-tree cp arcs polys)))))

;(defmethod create-tree ((cp shape))
;  (let* ((n (vertex-count cp))
	 

;;;; ---*--- 2D Collision Detection ---*---

;;; Intersection of two convex polygons.
(defun middle-index (i0 i1 n)
  "Find the vertex that bisects indices [i0,i1].  If i0=i1=0, then the returned
index is the one between [i0,N], where N is the last index in the polygon.  If
i1 < i0, then the middle index is found going clock-wise around the figure."
  (if (< i0 i1)
      (truncate (/ (+ i0 i1) 2))
      (mod (truncate (/ (+ i0 i1 n) 2)) n)))

(defmethod which-side (c p d)
  "The vertices of c are project onto the line P+t*D.  Returns +1
if all t > 0, -1 if t < 0, or 0 if the line splits the polygon."
  (let ((pos 0)
	(neg 0)
	(zer 0))
    (dotimes (i (vertex-count c))
      (let ((j (dot-product d (m- (vertex c i) p))))
	(cond ((> j 0.0) (incf pos))
	      ((< j 0.0) (incf neg))
	      ((= j 0.0 (incf zer))))))
    (cond ((or (and (> pos 0) (> neg 0)) (> zer 0)) 0)
	  ((> pos 0) 1)
	  (t -1))))

(defun extreme-index-slow (c d)
  "Find the extreme vertex of shape <c> along direction <d>."
  (with-slots ((n vertex-count)) c
    (iter (with i0 = 0)
	  (with i1 = 0)
	  (after-each (let ((mid (middle-index i0 i1 n)))
			(if (> (dot-product d (edge-vector c mid)) 0.0)
			    (if (not (= i0 mid))
				(setq i0 mid)
				(leave i1))
			    (if (< (dot-product d (edge-vector c (1- mid))) 0.0)
				(setf i1 mid)
				(leave mid))))))))

(defun extreme-index (c d)
  "Find the extreme vertex of shape <c> along direction <d> using a binary space partition."
  (iter (for node initially (bspace-partition c)
	     then (if (>= (dot-product (edge-vector c (bsp-value node)) d) 0.0)
		      (bsp-right node)
		      (bsp-left node)))
	(until (null (bsp-right node)))
	(finally node)))

(defmethod static-intersects-p ((c0 shape-2d) (c1 shape-2d))
  "Test for intersection between two resting shapes."
  (let ((n0 (vertex-count c0))
	(n1 (vertex-count c1)))
    (and 
     ;; Test separation for edges of c0
     (iter (for i1 from 0 below n0)
	   (for i0 previous i1 initially n0)
	   (never (let ((p (vertex c0 i1))
			(d (normal c0 i0)))
		    (> (dot-product d (m- (vertex c1 (extreme-index c1 (mneg d))) p)) 0.0))))
     ;; Test separation for edges of c1
     (iter (for i1 from 0 below n1)
	   (for i0 previous i1 initially n1)
	   (never (let ((p (vertex c1 i1))
			(d (normal c1 i0)))
		    (> (dot-product d (m- (vertex c0 (extreme-index c0 (mneg d))) p)) 0.0)))))))

(defmethod compute-interval ((cp shape) direction)
  (values (dot-product direction (vertex cp (extreme-index cp (mneg direction))))
	  (dot-product direction (vertex cp (extreme-index cp direction)))))

(defmethod static-intersects-p ((c0 shape) (c1 shape))
  "Test for intersection between two resting shapes."
  (and
   (iter (for d in-vector (normals c0)) ; Test faces of C0 for separation
	 (never (multiple-value-bind (min0 max0) (compute-interval c0 d)
		  (multiple-value-bind (min1 max1) (compute-interval c1 d)
		    (or (< max1 min0) (< max0 min1))))))
   (iter (for d in-vector (normals c1)) ; Test faces of C1
	 (never (multiple-value-bind (min0 max0) (compute-interval c0 d)
		  (multiple-value-bind (min1 max1) (compute-interval c1 d)
		    (or (< max1 min0) (< max0 min1))))))
   (iter (for e0_i in (edges c0)) ; Cross product of pairs of edges
	 (for e0 = (edge-vector c0 e0_i))
	 (never (iter (for e1_i in (edges c1))
		      (for e1 = (edge-vector c1 e1_i))
		      (for d = (cross e0 e1))
		      (never (multiple-value-bind (min0 max0) (compute-interval c0 d)
			       (multiple-value-bind (min1 max1) (compute-interval c1 d)
				 (or (< max1 min0) (< max0 min1))))))))))
	       
;;;; ---*--- Detecting 3D Intersections ---*---

(defmethod static-intersects-p ((b0 bounding-box) (b1 bounding-box))
  (with-slots ((b0-c c) (b0-u u) (b0-e e)) b0
    (with-slots ((b1-c c) (b1-u u) (b1-e e)) b1
      (let ((cutoff 0.999999d0)
	    (parallel-pair-p nil)
	    (diff (m- b1-c b0-c))
	    (c (make-array '(3 3)))
	    (abs-c (make-array '(3 3)))
	    (dot (make-array 3)))
	(flet ((test-ai-axis (idx)
		 (dotimes (i 3)
		   (setf (aref c idx i) (dot-product (aref b0-u idx)
						     (aref b1-u i))
			 (aref abs-c idx i) (abs (aref c idx i)))
		   (when (> (aref abs-c idx i) cutoff)
		     (setf parallel-pair-p t)))
		 (setf (aref dot idx) (dot-product diff (aref b0-u 0)))
		 (let ((r (abs (aref dot idx)))
		       (r0 (aref b0-e idx))
		       (r1 (+ (* (aref b1-e 0) (aref abs-c idx 0))
			      (* (aref b1-e 1) (aref abs-c idx 1))
			      (* (aref b1-e 2) (aref abs-c idx 2)))))
		   (<= r (+ r0 r1))))
	       (test-bi-axis (idx)
		 (let ((r (abs (dot-product diff (aref b1-e idx))))
		       (r0 (+ (* (aref b0-e 0) (aref abs-c 0 idx))
			      (* (aref b0-e 1) (aref abs-c 1 idx))
			      (* (aref b0-e 2) (aref abs-c 2 idx))))
		       (r1 (aref b1-e idx)))
		   (<= r (+ r0 r1))))
	       (test-aibi-axis (ia ib)
		 (let ((r (abs (- (* (aref dot 2) (aref c 1 ib))
				  (* (aref dot 1) (aref c 2 ib)))))
		       (r0 (+ (* (aref b0-e 1) (aref abs-c 2 ib))
			      (* (aref b0-e 2) (aref abs-c 1 ib))))
		       (r1 (+ (* (aref b1-e 1) (aref abs-c ia 2))
			      (* (aref b1-e 2) (aref abs-c ia 1)))))
		   (< r (+ r0 r1)))))
	  (and (test-ai-axis 0) (test-ai-axis 1) (test-ai-axis 2)
	       (test-bi-axis 0) (test-bi-axis 1) (test-bi-axis 2)
	       (or parallel-pair-p
		   (test-aibi-axis 0 0) (test-aibi-axis 0 1) (test-aibi-axis 0 2)
		   (test-aibi-axis 1 0) (test-aibi-axis 1 1) (test-aibi-axis 1 2)
		   (test-aibi-axis 2 0) (test-aibi-axis 2 1) (test-aibi-axis 2 2))))))))
;;;;

(defun test-unique-edge-set ()
  (let* ((faces (vector #(0 2 1)
			#(0 1 3)
			#(0 3 2)
			#(1 2 3)))
	 (edges (unique-edge-set faces)))
    (format t "The unique edge set for a tetrahedron is ~A" edges)
    edges))
    
(defun test-tetrahedron ()
  (make-tetrahedron (make-vector 0.0 0.0 0.0)
		    (make-vector 1.0 0.0 0.0)
		    (make-vector 0.0 1.0 0.0)
		    (make-vector 0.0 0.0 1.0)))

(defun test-adjacencies ()
  (let ((tetra (test-tetrahedron)))
    (format t "Faces adjacent to P0 are ~A.~%" (faces-adjacent-to-vertex tetra 0))
    (format t "Faces adjacent to P1 are ~A.~%" (faces-adjacent-to-vertex tetra 1))
    (format t "Faces adjacent to P2 are ~A.~%" (faces-adjacent-to-vertex tetra 2))
    (format t "Faces adjacent to P3 are ~A.~%" (faces-adjacent-to-vertex tetra 3))
    (format t "Faces adjacent to E0 (~A) are ~A.~%" (edge tetra 0) (faces-adjacent-to-edge tetra 0))
    ))

(defun test-space-partition ()
  (let ((tetra (test-tetrahedron)))
    (format t "Extreme index: ~A~%" (extreme-index tetra (make-vector 0.0 0.0 1.0)))
    (format t "Interval: ~A~%" (compute-interval tetra (make-vector 1.0 0.0 0.0)))
    tetra))

(defun test-shape-2d ()
  (let* ((v0 (make-vector -1.0 -1.0 0.0))
	 (v1 (make-vector 1.0 -1.0 0.0))
	 (v2 (make-vector 1.0 1.0 0.0))
	 (v3 (make-vector -1.0 1.0 0.0))
	 (poly (make-instance 'shape-2d :vertices (vector v0 v1 v2 v3))))
    (assert (and (vector= v0 (vertex poly 0))
		 (vector= v1 (vertex poly 1))
		 (vector= v2 (vertex poly 2))
		 (vector= v3 (vertex poly 3))))
    (assert (= 4 (vertex-count poly)))
    (assert (vector= v0 (vertex poly 4)))
    (assert (vector= v3 (vertex poly -1)))
    (assert (vector= (m- v1 v0) (edge poly 0)))
    (format t "~A~%" (normal poly 0))
    poly))
